#include "RASLib/init.h"
#include "RASLib/uart.h"
#include "pid.h"


float UpdatePID(SPid*pid, float err, float position) 
{
	float pTerm, dTerm, iTerm;


	//PRINT_I((int)pid->pGain); TAB;
	//PRINT_I((int)pid->iGain); TAB;
	//PRINT_I((int)pid->dGain); NL;
	PRINT_I((int)err); TAB;
	//PRINT_I((int)position); NL;

	pTerm = pid->pGain*err;

	pid->iState +=err;
	pid->iState = SATURATE(pid->iMin, pid->iMax, pid->iState);
	iTerm = pid->iGain*pid->iState;
	dTerm = pid->dGain*(position - pid->dState);
	pid->dState = position;

	PRINT_I((int)pTerm); NL;
	//PRINT_I((int)dTerm); TAB;
	//PRINT_I((int)iTerm); NL;
	return pTerm + iTerm - dTerm;
}
